'''
####
Date : 2025-1-8
Notes: UI界面显示
此脚本主要功能:
1. 界面设计-手动控制-状态监控-机器视觉-人机交互
2. 数据交互:机械手控制-示教, Modbus寄存器值进行变量监控
3. 机器视觉:原图显示-结果图显示
4. 人机交互

####

modify virtual controller
1. rtu_master注释掉 pass
2. 还原modbus python版本，虚拟时候不会出现卡顿现象
'''

# -*- coding: utf-8 -*-
import logging
import os
import sys
import cv2
import socket
import struct
import time
import modbus_tk.defines as cst
# import modbus_tk.modbus_tcp as modbus_tcp
import psutil
from threading import Thread

logging.getLogger("PyQt5.uic").setLevel(logging.WARNING)

### 导入Qt的封装函数类（python版本）
from PyQt5 import uic
from PyQt5.QtWidgets import QApplication, QMainWindow, QHeaderView
from PyQt5.QtGui import QFont, QImage, QPixmap, QIcon
from PyQt5.QtCore import Qt, QTimer, QAbstractTableModel

#### 导入相关类函数-- tcp通讯（modbus地址客户端）-- RTU通讯（拓展板IO状态交互）
#### 导入相关类函数-- driver_move（ZMove上位机通讯）-- visionCapture_（机器视觉-图像获取、图像处理类）
from processModbusTCP__ import process_Master
# from processModbusRTU_ import rtu_Master
from driver_move import VSRControler_Server
from visionCapture_ import Camera_Thread
import numpy as np

# C++编写的ModbusTCP客户端
# from TCPmodbus_wrapper import ModbusMaster

### 初始化对象--线程（避免主线程卡顿）VSR虚拟控制器
vsrserver = VSRControler_Server(simulate=True)
ser_thread = Thread(target=vsrserver._run)
ser_thread.start()

### 日志文件
robot_logger=logging.getLogger("ZMove")
robot_logger.setLevel(logging.DEBUG)
### 默认显示
os.environ['DISPLAY'] = ':0'

#### Z_Move示教软件复制粘贴此处，方便人机交互时候调用点位
# 使用偏移的方法控制机器人的时候可以只示教四轴六轴过渡点、六轴仓库1号取料上方点、六轴托盘1号放料上方点、
# 四轴取料起始点（外部取料上方点）四轴放料起始点（内部放料上方点）、
# 六轴托盘1号取料上方点、六轴仓库1号放料上方点
# 需要注意的是，使用偏移的时候需要设置偏移量，如下：
# 以下定义的内容仅在使用偏移的方法控制机器人的时候才用得到
# 定义六轴从仓库取料、放料到托盘和从托盘入库的x、y、z轴偏移量x_data_6、y_data_6、z_data_6以及1轴偏移量J1_data_6
# 取料时需要使用J1_data_6_1和z_data_6_1
J1_data_6_1 = -10
z_data_6_1 = -10
# 放料到托盘需使用x_data_6_2、y_data_6_2、z_data_6_2
x_data_6_2 = -10
y_data_6_2 = -10
z_data_6_2 = -10
# 入库需要使用x_data_6_3、y_data_6_3、z_data_6_3和J1_data_6_3
x_data_6_3 = -10
y_data_6_3 = -10
z_data_6_3 = -10
J1_data_6_3 = -10

# 定义四轴转运托盘的x、y、z轴偏移量x_data_4、y_data_4、z_data_4
x_data_4 = 10
y_data_4 = 10
z_data_4 = -10

# 示教时请务必只改位置数据不要改点位名称！！！
# 六轴过渡点（可用零点代替）
P_Translation=[0, 0, 0, 0, 0, 0, 198.6, 0, 230.72, 0, 0, 0, 0]
# 四轴过渡点（可用零点代替）
P_Translation_4=[0, 0, 0, 0, 0, 0, 195, 0, 269, 0, 0, 0, 0]
# 五个仓库取料点（可以尝试一下使用1轴偏移得到其余四个取料点， 而且取料点可以直接在上方点的基础上偏移z轴）
P1=[-23.9, 20.8, 3.0, 0.0, -25.2, 4.9, 211.8, -93.5, 153.6, 0.0, -1.3, -19, 0.0]
P2=[-35.2, 25.6, -2.5, 0.0, -23.1, -2.8, 195.9, -138.0, 152.3, 0.0, 0.0, -38.0, 0.0]
P3=[-46.9, 24.0, 0.1, 0.0,-24.1,46.9, 161.2, -172.5, 150.7, 0.0, 0.0, 0.0, 0.0]
P4=[-60.0, 16.9, 8.8, 0.0, -25.7,4.0, 111.1, -192.2, 150.7, 0.0, 0.0, -56.0, 0.0]
P5=[-81.64,7.372,11.953,0.0,-19.318,2.568,30.476,-207.395,172.779,0.0,0.0,-79.072,0.0]
# 五个仓库取料上方点（同样可以尝试一下使用1轴偏移得到其余四个取料上方点, 而且上方点可以直接在取料点的基础上偏移z轴）
P6=[-23.9, 18.4, -0.6, 0.0, -19.2, 4.9, 211.8, -93.5, 172.8, 0.0, -1.3, -19, 0.0]
P7=[-35.2, 22.4, -8.2, 0.0,-14.2, -2.8, 195.9, -138.0, 180.5, 0.0, 0.0, -38.0, 0.0]
P8=[-46.9, 21.1, -24.4, 0.0, 3.3,46.9, 161.2, -172.5, 233.2, 0.0, 0.0, -21.0, 0.0]
P9=[-60.0, 11.8, -5.9, 0.0,-6.0,4.0, 111.1, -192.2, 210.7, 0.0, 0.0, -56.0, 0.0]
P10=[-80.737,11.491,-11.234,0.0,-0.261,1.659,35.457,-217.403,227.798,0.0,0.0,-79.078,0.0]
# 4个托盘放料点（可以尝试一下使用1轴偏移得到其余四个放料点， 而且放料点可以直接在上方点的基础上偏移z轴）
P11=[14.4, 54.4, -22.7, 0.0, -31.7, 16.0,263.3, 67.4, 93.8, 0.0, 0.0, 30.45, 0.0]
P12=[6.4, 68.8, -45.9, 0.0, -22.9, 25.9, 292.0, 32.6,94.5, 0.0, 0.0, 32.3, 0.0]
P13=[2.8, 43.5, -6.6, 0.0, -36.9, -2.3,250.9, 12.4, 95.7, 0.0, 0.0, 0.5, 0.0]
P14=[9.2, 36.0, 7.6, 0.0, -43.5, 45.8,226.4, 36.7, 88.2, 0.0, 0.0, 55.0, 0.0]
# 4个托盘上方点（同样可以尝试一下使用1轴偏移得到其余四个放料上方点, 而且上方点可以直接在放料点的基础上偏移z轴）
P15=[13.7, 46.0, -27.5, 0.0, -18.5, 35.8, 266.2, 65.0, 143.2, 0.0, 0.0, 30.45, 0.0] 
P16=[6.4, 61.4, -60.3, 0.0, -1.1, 25.9, 292.0, 32.6, 171.3, 0.0, 0.0, 32.3, 0.0]
P17=[2.8, 33.0, -9.2, 0.0, -23.9,-2.3 ,250.9, 12.4, 143.2, 0.0, 0.0, 0.5, 0.0]
P18=[3.6, 3.6, 29.2, 0.0, -32.8,51.4 ,188.9, 11.7, 135.7, 0.0, 0.0, 55.0, 0.0]
# 四轴机器人八个点位
P19=[0.0,43.6,94.5,0.0,0.0,0.0,140.6,0.0,90.8,0.0,0.0,0.0,0.0]
P20=[66.8,69.2,101.0,25.1,0.0,0.0,62.8,146.3,41.3,25.1,0.0,0.0,0.0]
P21=[74.8,72.4,68.3,25.1,0.0,0.0,62.8,231.9,41.3,25.1,0.0,0.0,0.0]
P22=[74.8,44.2,51.2,25.1,0.0,0.0,62.8,231.9,120.3,25.1,0.0,0.0,0.0]  
P23=[-27.0,42.1,59.1,115.4,0.0,0.0,196.8,-100.3,112.7,115.4,0.0,0.0,0.0] 
P24=[-27.0,69.5,75.7,115.4,0.0,0.0,196.8,-100.3,42.3,115.4,0.0,0.0,0.0]
P25=[-44.6,79.6,53.2,115.4,0.0,0.0,196.8,-194.3,42.3,115.4,0.0,0.0,0.0]
P26=[-44.6,65.1,47.5,115.4,0.0,0.0,196.8,-194.3,84.7,115.4,0.0,0.0,0.0] 
# 4个托盘取料点（可以尝试一下使用1轴偏移得到其余四个取料点， 而且取料点可以直接在上方点的基础上偏移z轴）
P27=[42.3, 61.2, -33.5, 0.0, -27.7,-7.7, 209.4, 190.7, 93.9, 0.0, 0.0, 34.7, 0.0]
P28=[34.0,60.0, -31.7, 0.0, -28.3,0.2, 233.3, 157.4, 94.1, 0.0, 0.0, 34.2, 0.0]
P29=[42.5,36.6, 5.3, 0.0, -41.9,-11.1, 171.9, 157.6, 91.6, 0.0, 0.0, 31.4, 0.0]
P30=[32.0,36.3, 5.4, 0.0, -41.7,-3.1, 197.6, 123.6, 92.2, 0.0, 0.0, 28.9, 0.0]
# 4个托盘取料上方点（同样可以尝试一下使用1轴偏移得到其余四个取料上方点, 而且上方点可以直接在取料点的基础上偏移z轴）
P31=[42.3, 50.9, -40.1, 0.0,-10.8,-7.7, 209.4, 190.7, 158.8, 0.0, 0.0, 34.7, 0.0]
P32=[34.0, 50.2, -36.2, 0.0,-14.0,0.2, 233.3, 157.4, 150.5, 0.0, 0.0, 34.2, 0.0]
P33=[42.5, 18.8, -4.6, 0.0,-14.3,-11.1, 171.9, 157.6, 182.7, 0.0, 0.0, 31.4, 0.0]
P34=[32.0, 19.1, -3.5, 0.0,-15.6,-3.1, 197.6, 123.6, 178.7, 0.0, 0.0, 28.9, 0.0]
# 4个仓库放料点（可以尝试一下使用1轴偏移得到其余四个放料点， 而且放料点可以直接在上方点的基础上偏移z轴）
P35=[-24.4, 11.6, 34.2, 0.0, -45.9, 8.1,167.1, -75.8, 101.1, 0.0, 0.0, -16.3, 0.0]
P36=[-38.6, 13.1, 32.0, 0.0, -45.1, -4.5,146.6, -116.9, 102.2, 0.0, 0.0, -43.1, 0.0]
P37=[-52.4, 8.8, 36.7, 0.0, -45.6, -20.9,109.1, -141.7, 102.7, 0.0, 0.0, -73.3, 0.0]
P38=[-65.6, 1.1, 45.0, 0.0, -46.2, -33.8,67.4, -148.7, 102.6, 0.0, 0.0, -99.4, 0.0]
# 4个仓库放料上方点（同样可以尝试一下使用1轴偏移得到其余四个放料上方点, 而且上方点可以直接在放料点的基础上偏移z轴）
P39=[-24.4, 0.4, 32.3, 0.0, -32.7,8.1 ,167.1, -75.8, 136.3, 0.0, 0.0, -16.3, 0.0]
P40=[-38.6, -1.3, 27.6, 0.0, -26.3,-4.5 ,146.6, -116.9, 153.7, 0.0, 0.0, -43.1, 0.0]
P41=[-52.4, -3.4, 34.1, 0.0, -30.7,-20.9 ,109.1, -141.7, 141.4, 0.0, 0.0, -73.3, 0.0]
P42=[-67.4, -13.3, 12.5, 0.0, -28.2,-33.8 ,67.4, -148.7, 145.5, 0.0, 0.0, -99.4, 0.0]

P = np.array([P1,P2,P3,P4,P5, P6,P7,P8,P9,P10, P11,P12,P13,P14, P15,P16,P17,P18, P19,P20,P21,P22,P23,P24,P25,P26, P27,P28,P29,P30, P31,P32,P33,P34, P35,P36,P37,P38, P39,P40,P41,P42])

# 定义从托盘取料时各个取料点的中心坐标，依次为1号(x_lower,y_upper)、2号(x_upper,y_upper)、3号(x_lower,y_lower)、4号(x_lower,y_lower)
x_upper = 370
x_lower = 150
y_upper = 558
y_lower = 310

arm = vsrserver ###（虚拟控制器控制类内包含机械臂运行路径规划及控制）

### 为了让在使用python代码打开气泵的时候能够同步到IOA中，请使用下面定义的这个函数来开关气泵，如果不需要使用IOA则不必理会
# def set_DO(num,value):
#     try:
#         rtu_master.set_DO(num,value)
#         arm.bo_out = arm.bo_count(arm.bo_out,num,value=value)
    
#     except Exception as e:
#         print(e)

### 平替到虚拟控制器
def set_DO(num,value):
    try:
        arm.SetDO(num, value)
    
    except Exception as e:
        print(e)

class MainWindow(QMainWindow):

#### 用户自己修改人机交互控制部分
#### 人机交互-------------------------开始 ####

    #### 控制机器人动作的任务判断（用户自定义）-----********------重要代码部分
    #### 控制机器人动作的任务判断（用户自定义）-----********------重要代码部分
    #### 控制机器人动作的任务判断（用户自定义）-----********------重要代码部分
    ## 机器人A运动， com_Num = 1
    def robotTask(self, task_id):
        try:
            if task_id == 1:
                # arm.set_joint_angle(com_Pose=P6, com_Num=1)
                # arm.set_joint_angle(com_Pose=P1, com_Num=1)
                arm.robot_home(com_Num=1)

            elif task_id == 2:
                j = 0
                for i in range(4): 
                    print(f"正在处理{i+1}号仓位")
                    
                    # 取料动作序列
                    arm.set_joint_angle(com_Pose=P[i + 5], com_Num=1)  # 移动到取料过渡点
                    arm.set_joint_angle(com_Pose=P[i], com_Num=1)       # 移动到取料点
                    time.sleep(0.5)
                    set_DO(0, 1)  # 执行取料（如夹爪闭合）
                    time.sleep(0.5)
                    arm.set_joint_angle(com_Pose=P[i + 5], com_Num=1)  # 退回取料过渡点
                    arm.set_joint_angle(com_Pose=P_Translation, com_Num=1)  # 退回安全位置

                    # 放置动作序列（确保只放置4次）
                    arm.set_joint_angle(com_Pose=P[j + 14], com_Num=1)  # 移动到放置过渡点
                    arm.set_joint_angle(com_Pose=P[j + 10], com_Num=1)  # 移动到放置点
                    time.sleep(0.5)
                    set_DO(0, 0)  # 执行放料（如夹爪张开）
                    time.sleep(0.5)
                    arm.set_joint_angle(com_Pose=P[j + 14], com_Num=1)  # 退回放置过渡点
                    arm.set_joint_angle(com_Pose=P_Translation, com_Num=1)  # 退回安全位置
                    
                    print(f"已将{i+1}号仓位物料放置在托盘{j+1}号位置")
                    j += 1  # 更新放置位置计数器

                print("托盘已放满4个物料！")
            
            elif task_id == 3:
                # 拍一张照片
                self.camera_thread.stop = False
                time.sleep(0.5)
                self.camera_thread.stop = True
                time.sleep(0.5)
                # 检测二维码
                self.camera_thread.detect_qr_code = 1
                time.sleep(2)

            elif task_id == 4:
                def get_material_position(x,y):
                    try:
                        value = 0
                        if np.fabs(x - x_upper) < 50 and np.abs(y - y_lower) < 50:
                            value = 1
                        elif np.fabs(x - x_upper) < 50 and np.abs(y - y_upper) < 50:
                            value = 2
                        elif np.fabs(x - x_lower) < 50 and np.abs(y - y_lower) < 50:
                            value = 3
                        elif np.fabs(x - x_lower) < 50 and np.abs(y - y_upper) < 50:
                            value = 4
                        return value
                    except BaseException as e:
                        print(f"托盘取料值获取错误{e}")
                
                result = self.camera_thread.result_message
                length = len(result)
                if length == 0:
                    print("未检测到任何物料")
                else:
                    # 定义视觉检测结果中的类型变量和对应位置
                    category = 0
                    x = y = 0
                    for i in range(length):
                        if result[i][0] in {"1", "A", "red"}:
                            category = 1
                        elif result[i][0] in {"2", "B", "green"}:
                            category = 2
                        elif result[i][0] in {"3", "C", "blue"}:
                            category = 3
                        elif result[i][0] in {"4", "D", "yellow"}:
                            category = 4
                        
                        if category != 0:
                            x = result[i][1][0]
                            y = result[i][1][1]
                            print(f"x:{x},y:{y}")
                            value = get_material_position(x,y)
                            if value != 0:
                                arm.set_joint_angle(com_Pose=P[value-1+30], com_Num=1)
                                arm.set_joint_angle(com_Pose=P[value-1+26], com_Num=1)
                                time.sleep(0.5)
                                set_DO(0,1)
                                time.sleep(0.5)
                                arm.set_joint_angle(com_Pose=P[value-1+30], com_Num=1)
                                arm.set_joint_angle(com_Pose=P_Translation, com_Num=1)
                                arm.set_joint_angle(com_Pose=P[category-1+38], com_Num=1)
                                arm.set_joint_angle(com_Pose=P[category-1+34], com_Num=1)
                                time.sleep(0.5)
                                set_DO(0,0)
                                time.sleep(0.5)
                                arm.set_joint_angle(com_Pose=P[category-1+38], com_Num=1)
                                arm.set_joint_angle(com_Pose=P_Translation, com_Num=1)
                                print(f"已将托盘{value}号位置的二维码{category}成功入库")
                                category = 0
                            else:
                                print("未检测到符合位置物料")
                        else:
                            print("没有检测到所需物料")
            
        except BaseException as e:
            robot_logger.error(e,exc_info=True)


    #### 控制机器人动作的任务判断（用户自定义第二行）
    ## 机器人B运动
    def robotTask2(self, task_id):
        try:
            if task_id == 1:
                arm.robot_home(com_Num=2)
            
            elif task_id == 2:
                for i in range(8):
                    arm.set_joint_angle(com_Pose=P[18 + i], com_Num=2)
                print("四轴转运完成，准备打开传送带")
                time.sleep(0.5)
                set_DO(1,1)
                time.sleep(6.25)
                set_DO(1,0)
                time.sleep(0.5)
                print("物料已到达视觉检测点")
            
        except BaseException as e:
            robot_logger.error(e,exc_info=True)


    def robot2Task(self, task_id):
        try:
            if task_id == 1:
                print(1)

            elif task_id == 2:
                print(2)
            
            elif task_id == 3:
                print(3)

            elif task_id == 4:
                print(4)
        except BaseException as e:
            robot_logger.error(e,exc_info=True)

    def robot2Task2(self, task_id):
        try:
            if task_id == 1:
                print(11)

            elif task_id == 2:
                print(12)
            
            elif task_id == 3:
                print(13)

            elif task_id == 4:
                print(14)
        except BaseException as e:
            robot_logger.error(e,exc_info=True) 

    def autoworking(self):
        try:
            ### 六轴复位
            self.robotTask(1)
            ### 四轴复位
            self.robotTask2(1)
            ### 出仓--托盘
            self.robotTask(2)
            ### 四轴转运货
            self.robotTask2(2)
            ### 物料视觉检测
            self.robotTask(3) 
            ### 托盘取料回仓
            self.robotTask(4)
        
        except Exception as e:
            print(f"autoworking error is {e}")

    def run_task(self, task_method, task_id):
        try:
            task_method(task_id)
        finally:
            self.on_task_finished()

    def on_task_finished(self):
        # 任务结束后界面刷新
        self.label_AlarmInfo.setText("程序空闲中")

    def connect_button(self, button, task_method, task_id):
        def on_click():
            self.label_AlarmInfo.setText("程序运行中")
            # 创建线程
            thread = Thread(target=self.run_task, args=(task_method, task_id))
            thread.start()

        button.clicked.connect(on_click)

    #### 初始化按钮，以及对应的槽函数（任务ID）
    def taskPushButtonInit(self):
        ## 用户自定义任务按钮
        taskButtons = {
            self.pushButton_taskPage1_task1: 1,
            self.pushButton_taskPage1_task2: 2,
            self.pushButton_taskPage1_task3: 3,
            self.pushButton_taskPage1_task4: 4,
        }
        for button, task_id in taskButtons.items():
            self.connect_button(button, self.robotTask, task_id)

        ## 用户自定义任务按钮（第二行）
        taskButtons2 = {
            self.pushButton_taskPage1_task1_2: 1,
            self.pushButton_taskPage1_task2_2: 2,
        }

        for button, task_id in taskButtons2.items():
            self.connect_button(button, self.robotTask2, task_id)
        
        ## 页面切换按钮page1,page2
        pageButtons = {
            self.pushButton_taskPage1: 0,
            self.pushButton_taskPage2: 1,
        }

        for button, page_index in pageButtons.items():
            button.clicked.connect(lambda checked, index=page_index: self.stackedWidget_task.setCurrentIndex(index))
        
        ### 第二页用户自定义按钮第一行
        task2Buttons = {
            self.pushButton_taskPage2_task1: 1,
            self.pushButton_taskPage2_task2: 2,
            self.pushButton_taskPage2_task3: 3,
            self.pushButton_taskPage2_task4: 4,
        }
        for button, task_id in task2Buttons.items():
            self.connect_button(button, self.robot2Task, task_id)

        ### 第二页用户自定义按钮第二行
        task2Buttons2 = {
            self.pushButton_taskPage2_task1_2: 1,
            self.pushButton_taskPage2_task2_2: 2,
            self.pushButton_taskPage2_task3_2: 3,
            self.pushButton_taskPage2_task4_2: 4,
        }
        for button, task_id in task2Buttons2.items():
            self.connect_button(button, self.robot2Task2, task_id)

        ## 界面按钮：急停，机器人A、B复位，自动模式
        uiButtons = {
            self.pushButton_customizeStop: 1,
            self.pushButton_customizeRobotA_Home: 2,
            self.pushButton_customizeRobotB_Home: 3,
            self.pushButton_auto_manually: 4,
        }
        
        for button, task_id in uiButtons.items():
            button.clicked.connect(lambda checked, task_id=task_id: self.robotCMD(task_id))
    

    def robotCMD(self, task_id):
        try:
            # robot1_type = server.vsrserver.get_robot_type("robot_type")
            # robot2_type = server.vsrserver.get_robot_type("robot_type2")
            if task_id == 1:
                arm.robot_stop()

            elif task_id == 2:
                # if robot1_type != "未连接":
                    arm.robot_home(com_Num=1)
            
            elif task_id == 3:
                # if robot2_type != "未连接":
                    arm.robot_home(com_Num=2)
            elif task_id == 4:
                self.autoworking()
            
        except BaseException as e:
            robot_logger.error(e,exc_info=True)

#### 人机交互-------------------------结束 ####



    def __init__(self):
        super().__init__()
        self.initParameters()
        self.setup_ui()
        self.msg_history = list()
        self.msgVisionPara_history = list()
        self.msgVisonRes_history = list()
        self.init_pushButtonSignals()
        self.init_labelSingls()
        self.set_initial_size()
        self.robot_control_thread()
        
        ### KBot网络通信请求更新线程
        self.KBot_Request_thread = Thread(target=self.KBot_request)
        self.KBot_Request_thread.start()

        ### 机械手信息更新线程
        self.robotInfo_thread = Thread(target=self.init_labelRobotAB)
        self.robotInfo_thread.start()
        self.init_DOpushButton()

        #### 视觉显示线程
        self.camera_thread = Camera_Thread(self.label_oriShow)
        self.camera_thread.start()

        self.cameraUpdate_timer = QTimer(self)
        self.cameraUpdate_timer.timeout.connect(self.visionUpdateFrame)

        ### 视觉显示
        self.vision_initSingal()

        #### 变量监控页面显示
        #### new
        self.dataPageInit()
        self.setup_pages('bool')
        self.setup_pages('int')
        self.setup_pages('float')
        self.setup_pages('pose')

        self.modbus_clients_already = []

        self.timer = QTimer(self)
        self.timer.timeout.connect(self.KBot_state)
        self.timer.timeout.connect(self.valueMonitor)
        self.timer.timeout.connect(self._initIOValuesMonitor)
        self.timer.timeout.connect(self.getModbusClient)
        self.timer.start(1000) 
        
        #### 人机交互页面显示
        ## 自定义任务按钮初始化
        self.taskPushButtonInit()

    def KBot_request(self):
        while True:
            KBot_Flag = master.get_hold_register(181, 1)[0]
            # KBot_Flag = master.read_holding_registers(1, 181, 1)[0]
            if KBot_Flag == 3:
                master.master_.execute(1, cst.WRITE_SINGLE_REGISTER, starting_address=181, output_value=1)
                self.robotTask(3)
                self.robotTask(4)
            time.sleep(0.05)

    ### 初始化参数
    def initParameters(self):
        self.robotA_type = None
        self.robotB_type = None
        ### 机器人运动步进、速度
        self.step = 5
        self.speed = 2000
    
    ### 初始化UI显示位置及尺寸
    def set_initial_size(self):
        self.setGeometry(1, 25, 1280, 640)
    
    ### 加载qt designer文件
    def setup_ui(self):
        current_dir = os.path.dirname(os.path.abspath(__file__))
        ui_file_path = os.path.join(current_dir, "..", "ui", "mainWindow.ui")
        uic.loadUi(ui_file_path, self)
        self.setWindowTitle("ZBot")
        self.setWindowIcon(QIcon('../img/icon.png'))

    ### 初始化UI按钮信号
    def init_pushButtonSignals(self):
        try:
            # Mapping buttons to their corresponding stacked widget indices
            ### 主界面按钮--以及对应的页面
            button_to_page = {
                self.pushButton_tpu: 1,
                self.pushButton_monitor: 2,
                self.pushButton_vision: 3,
                self.pushButton_tcp: 4,
                self.pushButton_comeBack: 0,
                self.pushButton_comeBack_2: 0,
                self.pushButton_comeBack_3: 0,
                self.pushButton_comeBack_4: 0,

            }

            # def pushButtonStyle(page_index):
            #     if page_index == 1 :
            #         self.pushButton_tpu.setStyleSheet("background-color: rgb(255, 129, 66);")
            #     elif page_index == 2:
            #         self.pushButton_monitor.setStyleSheet("background-color:  rgb(16, 200, 255);")
            #     elif page_index == 3 :
            #         self.pushButton_vision.setStyleSheet("background-color: rgb(21, 96, 130);")
            #     elif page_index == 4:
            #         self.pushButton_tcp.setStyleSheet("background-color:  rgb(25, 107, 36);")

            for button, page_index in button_to_page.items():
                button.clicked.connect(lambda checked, index=page_index: self.stackedWidget.setCurrentIndex(index))
                # button.clicked.connect(lambda checked, index=page_index: pushButtonStyle(index))
                if page_index == 1 :
                    self.pushButton_tpu.setStyleSheet("background-color: rgb(255, 129, 66);")
                elif page_index == 2:
                    self.pushButton_monitor.setStyleSheet("background-color:  rgb(16, 200, 255);")
                elif page_index == 3 :
                    self.pushButton_vision.setStyleSheet("background-color: rgb(21, 96, 130);")
                elif page_index == 4:
                    self.pushButton_tcp.setStyleSheet("background-color:  rgb(25, 107, 36);")

                # self.stop_qr_detection()

            
            ### 手动控制页面按钮--对应的机器人示教界面
            robot_button_to_page = {
                self.pushButton_pageArobot: 0,
                self.pushButton_pageBrobot: 1,
            }

            for butt, page_ind in robot_button_to_page.items():
                butt.clicked.connect(lambda checked, index=page_ind: self.stackedWidget_robot.setCurrentIndex(index))
            

            ### 手动控制页面侧面按钮--运动控制--IO控制
            control_button = {
                self.pushButton_robotContorl: 0,
                self.pushButton_ioControl: 1,
            }
            
            for butt, page_ind in control_button.items():
                butt.clicked.connect(lambda checked, index=page_ind: self.stackedWidget_2.setCurrentIndex(index))

            self.pushButton_close.clicked.connect(self.close)
            self.pushButton_close_2.clicked.connect(self.close)
            self.pushButton_close_3.clicked.connect(self.close)
            self.pushButton_close_4.clicked.connect(self.close)

            self.mointor_pushButton()

        except Exception as e:
            print(f"init_pushButtonSingnals error is {e}")

    ### 机器人控制 示教--复位--回零--急停
    def robot_control_thread(self):
        self.robotA_jointTeach()
        self.robotA_coordinateTeach()
        self.robotB_jointTeach()
        self.robotB_coordinateTeach()
        self.cmd_robot()

    ### 初始化UI标签显示
    ######################此方法的更新适用于所有界面的状态显示
    def init_labelSingls(self):
        ### 连接IP地址（本地）
        IP_address = self.get_local_ip()
        self.label_IPaddress.setText(IP_address)
        self.label_IPaddress_2.setText(IP_address)
        self.label_IPaddress_3.setText(IP_address)
        self.label_IPaddress_4.setText(IP_address)
        self.label_IPaddress_5.setText(IP_address)
        
    def KBot_state(self):
        pass
        ### KBot软件连接状态
        # print(f"server.vsrserver.KBotConnectStatus {server.vsrserver.KBotConnectStatus}")
        # if arm.:
        #     self.label_connectKBot.setText("已连接")
        #     self.label_connectKBot_2.setText("已连接")
        #     self.label_connectKBot_3.setText("已连接")
        #     self.label_connectKBot_4.setText("已连接")
        #     self.label_connectKBot_5.setText("已连接")
        # else:
        #     self.label_connectKBot.setText("未连接")
        #     self.label_connectKBot_2.setText("未连接")
        #     self.label_connectKBot_3.setText("未连接")
        #     self.label_connectKBot_4.setText("未连接")
        #     self.label_connectKBot_5.setText("未连接")

    ### 打印视觉结果信息
    def logVisionResult(self, msg):
        msg = time.strftime('%Y-%m-%d %H:%M:%S', time.localtime(time.time())) + " 视觉检测结果: " +  msg
        self.msg_history.append(msg)
        self.textBrowser_visionResultInfo.setText("".join(self.msg_history))
        self.textBrowser_visionResultInfo.moveCursor(self.textBrowser_visionResultInfo.textCursor().End)
        self.textBrowser_visionResultInfo.repaint()

    ### 打印视觉参数信息
    def logVisionParameter(self, msg):
        msg =" 视觉参数: \n" +  msg
        self.msgVisionPara_history.append(msg)
        self.textBrowser_visionParam.setText("".join(self.msgVisionPara_history))
        self.textBrowser_visionParam.moveCursor(self.textBrowser_visionParam.textCursor().End)
        self.textBrowser_visionParam.repaint()
    
    ### 打印modbusClient连接信息
    def logModbusClient(self, IP, Port):
        msg = "ModBus客户端IP: " +  IP + " 端口Port: " + Port
        self.textBrowser_modbusClient.append(msg)
        self.textBrowser_modbusClient.moveCursor(self.textBrowser_modbusClient.textCursor().End)
        self.textBrowser_modbusClient.repaint()

    def getModbusClient(self):
        modbus_clients = arm.modbusClients
        pass
        # modbus_clients_IP = arm.server.received_IP
        # modbus_clients_Port = arm.server.received_Port
        # if modbus_clients_IP is not None and modbus_clients_IP not in self.modbus_clients_already:
        #     self.modbus_clients_already.append(modbus_clients_IP)
        #     self.logModbusClient(modbus_clients_IP, modbus_clients_Port)
    
    ### 获取本地IP地址
    def get_local_ip(self):
        for interface, addrs in psutil.net_if_addrs().items():
            for addr in addrs:
                if addr.family == socket.AF_INET and not addr.address.startswith("127."):
                    return addr.address
        return None

    '''
    示例代码：
        1. UI数据绑定
        2. 示教控制代码:412行 手动控制界面-----Start  ---->  887 行 手动控制界面-----End
        3. 状态监控界面： 732行---Start   957行---End
    '''

    ### 数值转换函数--读取寄存器值，转换为float用于显示
    def int2float(self,a,b):
        value=0
        try:
            z0=hex(a)[2:].zfill(4) #取0x后边的部分 右对齐 左补零
            z1=hex(b)[2:].zfill(4) #取0x后边的部分 右对齐 左补零
            z=z0+z1 #高字节在前 低字节在后
            value=struct.unpack('!f', bytes.fromhex(z))[0] #返回浮点数
            if value is None:
                return value
            if isinstance(value, float):
                # 精确到小数点后两位数
                return round(value , 2)
            else:
                return value
        except BaseException as e:
            robot_logger.error(e,exc_info=True)
        return value

#############################
####### 手动控制界面-----Start
    ###-- 1. 机器人类型：KT-R6（6轴机器人）， KT-R4
    ###-- 2. 机器人状态： 离线--空闲-- 锁定--复位--运行
    def init_labelRobotAB(self):
        try:
            while True:
                self.robotA_type = arm.get_robot_type(1)
                self.robotB_type = arm.get_robot_type(2)
                self.label_robotAtype.setText(self.robotA_type)
                self.label_robotBtype.setText(self.robotB_type)
                self.label_robot1Connect.setText(self.robotA_type)
                self.label_robot2Connect.setText(self.robotB_type)
                self.label_robot1Connect_2.setText(self.robotA_type)
                self.label_robot2Connect_2.setText(self.robotB_type)
                self.label_robot1Connect_3.setText(self.robotA_type)
                self.label_robot2Connect_3.setText(self.robotB_type)
                self.label_robot1Connect_4.setText(self.robotA_type)
                self.label_robot2Connect_4.setText(self.robotB_type)
                self.label_robot1Connect_5.setText(self.robotA_type)
                self.label_robot2Connect_5.setText(self.robotB_type)

                # state_val = process_master.master_.execute(1, cst.READ_HOLDING_REGISTERS, 0, 1)[0]
                state_val = arm.robotA_status
                robotA_state = state_val
                state_val1 = arm.robotB_status
                robotB_state = state_val1  
                # if state_val == 0:
                #     robotA_state = "离线"
                # elif state_val == 1:
                #     robotA_state = "空闲"
                # elif state_val == 2:
                #     robotA_state = "锁定"
                # elif state_val == 3:
                #     robotA_state = "复位"
                # elif state_val == 4:
                #     robotA_state = "运行"
                # else:
                #     robotA_state = "未知"
                
                # state_val = process_master.master_.execute(1, cst.READ_HOLDING_REGISTERS, 60, 1)[0]
                # state_val = master.read_holding_registers(1, 60, 1)[0]
                # if state_val == 0:
                #     robotB_state = "离线"
                # elif state_val == 1:
                #     robotB_state = "空闲"
                # elif state_val == 2:
                #     robotB_state = "锁定"
                # elif state_val == 3:
                #     robotB_state = "复位"
                # elif state_val == 4:
                #     robotB_state = "运行"

                self.label_robotAstatus.setText(robotA_state)
                self.label_robotBstatus.setText(robotB_state)
                self.label_robotAstatus1.setText(robotA_state)
                self.label_robotBstatus1.setText(robotB_state)
                self.label_robotAstatus2.setText(robotA_state)
                self.label_robotBstatus2.setText(robotB_state)
                self.label_robotAstatus3.setText(robotA_state)
                self.label_robotBstatus3.setText(robotB_state)
                self.label_robotAstatus4.setText(robotA_state)
                self.label_robotBstatus4.setText(robotB_state)

                self.joint_coordinateShow()

        except Exception as e:
            robot_logger.error(e,exc_info=True)

    def init_DOpushButton(self):
        self.do_buttons = {
            self.pushButton_toggle_do_0: 0,
            self.pushButton_toggle_do_1: 1,
            self.pushButton_toggle_do_2: 2,
            self.pushButton_toggle_do_3: 3,
            # self.pushButton_toggle_do_4: 4,
            # self.pushButton_toggle_do_5: 5,
            # self.pushButton_toggle_do_6: 6,
            # self.pushButton_toggle_do_7: 7,
        }
        
        # Initialize a dictionary to keep track of each button's state.
        self.button_states = {index: False for index in self.do_buttons.values()}

        for butt, page_ind in self.do_buttons.items():
            butt.clicked.connect(lambda checked, index=page_ind: self.on_toggle(index, checked))

        self.motor_buttons = {
            self.pushButton_forward: 1,
            self.pushButton_inverse: 2,
            self.pushButton_Motor_start: 3,
            self.pushButton_Motor_stop:0,
        }
    
        for butt, page_ind in self.motor_buttons.items():
            butt.clicked.connect(lambda checked, index=page_ind: self.on_motorControl(0,index))

        self.motor_buttons_1 = {
            self.pushButton_forward_4: 1,
            self.pushButton_inverse_4: 2,
            self.pushButton_Motor_start_4: 3,
            self.pushButton_Motor_stop_4:0,
        }
    
        for butt, page_ind in self.motor_buttons_1.items():
            butt.clicked.connect(lambda checked, index=page_ind: self.on_motorControl(1,index))

    def on_toggle(self, index, checked):
        # Update the state for the specific button.
        self.button_states[index] = checked
        
        button = [butt for butt, idx in self.do_buttons.items() if idx == index][0]
        # button.setText("ON" if checked else "OFF")
        if checked:
            button.setText("ON")
            button.setStyleSheet("background-color: green;")
            arm.SetDO(index, 1)
            # pass
            # rtu_master.rtu_server.execute(1, cst.WRITE_SINGLE_REGISTER, starting_address = index + 12, output_value= 1)
        else:
            button.setText("OFF")
            button.setStyleSheet("background-color: white;")
            arm.SetDO(index, 0)
            # pass
            # rtu_master.rtu_server.execute(1, cst.WRITE_SINGLE_REGISTER, starting_address = index + 12, output_value= 0)

        # print(f"Switch {index} is {'ON' if checked else 'OFF'}")

    def motor_color(self,motor,action):
        if motor == 0:
            if action == 1:
                self.pushButton_forward.setStyleSheet("background-color: green;")
                self.pushButton_inverse.setStyleSheet("background-color: white;")
                self.pushButton_Motor_start.setStyleSheet("background-color: white;")
                self.pushButton_Motor_stop.setStyleSheet("background-color: white;")
            elif action == 2:
                self.pushButton_forward.setStyleSheet("background-color: white;")
                self.pushButton_inverse.setStyleSheet("background-color: green;")
                self.pushButton_Motor_start.setStyleSheet("background-color: white;")
                self.pushButton_Motor_stop.setStyleSheet("background-color: white;")
            elif action == 3:
                self.pushButton_forward.setStyleSheet("background-color: white;")
                self.pushButton_inverse.setStyleSheet("background-color: white;")
                self.pushButton_Motor_start.setStyleSheet("background-color: green;")
                self.pushButton_Motor_stop.setStyleSheet("background-color: white;")
            else:
                self.pushButton_forward.setStyleSheet("background-color: white;")
                self.pushButton_inverse.setStyleSheet("background-color: white;")
                self.pushButton_Motor_start.setStyleSheet("background-color: white;")
                self.pushButton_Motor_stop.setStyleSheet("background-color: green;")
        elif motor in [1, 2, 3]:  
            flag = 5 - motor
            if action == 1:  
                getattr(self, f'pushButton_forward_{flag}').setStyleSheet("background-color: green;")  
                getattr(self, f'pushButton_inverse_{flag}').setStyleSheet("background-color: white;")  
                getattr(self, f'pushButton_Motor_start_{flag}').setStyleSheet("background-color: white;")  
                getattr(self, f'pushButton_Motor_stop_{flag}').setStyleSheet("background-color: white;")  
            elif action == 2:  
                getattr(self, f'pushButton_forward_{flag}').setStyleSheet("background-color: white;")  
                getattr(self, f'pushButton_inverse_{flag}').setStyleSheet("background-color: green;")  
                getattr(self, f'pushButton_Motor_start_{flag}').setStyleSheet("background-color: white;")  
                getattr(self, f'pushButton_Motor_stop_{flag}').setStyleSheet("background-color: white;")  
            elif action == 3:  
                getattr(self, f'pushButton_forward_{flag}').setStyleSheet("background-color: white;")  
                getattr(self, f'pushButton_inverse_{flag}').setStyleSheet("background-color: white;")  
                getattr(self, f'pushButton_Motor_start_{flag}').setStyleSheet("background-color: green;")  
                getattr(self, f'pushButton_Motor_stop_{flag}').setStyleSheet("background-color: white;")  
            else:
                getattr(self, f'pushButton_forward_{flag}').setStyleSheet("background-color: white;")  
                getattr(self, f'pushButton_inverse_{flag}').setStyleSheet("background-color: white;")  
                getattr(self, f'pushButton_Motor_start_{flag}').setStyleSheet("background-color: white;")  
                getattr(self, f'pushButton_Motor_stop_{flag}').setStyleSheet("background-color: green;")  

    def on_motorControl(self, flag, index):
        self.motor_color(flag,index)
        pass
        # rtu_master.rtu_server.execute(1, cst.WRITE_SINGLE_REGISTER, starting_address = 16+flag, output_value= index)

    ###-- 3. 机器人坐标轴角、笛卡尔显示： joint_coordinateShow
    ###-- 4. 外层函数_get_robot_pose， 不同机器人取地址不同get_pose_floats， 寄存器读取姿态：read_pose_floats
    def read_pose_floats(self, starting_address):
        motion = master.master_.execute(1, cst.READ_HOLDING_REGISTERS, starting_address, 26)
        # motion = master.read_holding_registers(1, starting_address, 26)
        if not motion:
            return []

        floats = []
        for i in range(0, 26, 2):
            if i + 1 < len(motion):
                floats.append(self.int2float(motion[i], motion[i+1]))
            else:
                print("Error: Not enough integers to convert to floats.")
                break
        return floats

    def get_pose_floats(self, robotNum):
        try:
            if robotNum == 1:
                robot1_pose = self.read_pose_floats(1)
                return robot1_pose
            elif robotNum == 2:
                robot2_pose = self.read_pose_floats(61)
                return robot2_pose

        except Exception as e:
            print(e)

    def _get_robot_pose(self, robot_id):  
        try:  
            pose = self.get_pose_floats(robot_id)  
            return pose 
         
        except Exception as e:  
            return f"Error: {str(e)}"  

    def joint_coordinateShow(self):
        try:
            robot_pose = self._get_robot_pose(1)
            joints = robot_pose[:6]    
            coords = robot_pose[6:12]  
    
            for i, joint in enumerate(joints, 1):
                getattr(self, f'label_J{i}show').setText(str(joint))
            for i, coord in enumerate(coords, 1):
                getattr(self, f'label_C{i}show').setText(str(coord))
            
            # # 如果机器人有第7个关节 
            # if len(robot_pose) > 12:  
            #     getattr(self, f'label_J7').setText(str(robot_pose[12]))

            ### 虚拟控制器注释
            robot_pose_2 = self._get_robot_pose(2)
            joints_2 = robot_pose_2[:6]    
            coords_2 = robot_pose_2[6:12]  
    
            for j, joint_2 in enumerate(joints_2, 1):  
                getattr(self, f'label_J{j}show_2').setText(str(joint_2))
            for j, coord_2 in enumerate(coords_2, 1):
                label_name = ['X', 'Y', 'Z', 'A', 'B', 'C'][j - 1]  # 通过 i-1 获取标签名称  
                getattr(self, f'label_{label_name}_2').setText(str(coord_2))
        
        except Exception as e:
            robot_logger.error(e,exc_info=True)

    ###-- 5. A机器人控制：robotA_jointTeach轴角   robotA_coordinateTeach笛卡尔示教
    def robotA_jointTeach(self):
        try:
            joint_Pushbutton = {
                self.pushButton_J1min: 1,
                self.pushButton_J2min: 2,
                self.pushButton_J3min: 3,
                self.pushButton_J4min: 4,
                self.pushButton_J5min: 5,
                self.pushButton_J6min: 6,
                self.pushButton_J1add: 7,
                self.pushButton_J2add: 8,
                self.pushButton_J3add: 9,
                self.pushButton_J4add: 10,
                self.pushButton_J5add: 11,
                self.pushButton_J6add: 12,
            }
            
            # for butt, page_ind in joint_Pushbutton.items():
            #     butt.clicked.connect(lambda checked, index=page_ind: self.jointControRobotA(index))
            for butt, page_ind in joint_Pushbutton.items():
                # 连接 pressed 信号到第一个函数
                butt.pressed.connect(lambda checked=False, index=page_ind: self._jointControRobotA(index))
                # 连接 released 信号到第二个函数
                butt.released.connect(lambda checked=False, index=page_ind: self.jointControRobotRelease(index))
        
        except Exception as e:
            robot_logger.error(e,exc_info=True)
    
    ### 虚拟控制器槽函数
    def _jointControRobotA(self, index):
        self.step = 5.0
        if index == 1:
            val = arm.current_poseRobotA[0]
            arm.current_poseRobotA[0] = val - self.step
            arm.set_joint_angle(com_Pose=arm.current_poseRobotA, com_Num=1)
        elif index ==2:
            val = arm.current_poseRobotA[1]
            arm.current_poseRobotA[1] = val - self.step
            arm.set_joint_angle(com_Pose=arm.current_poseRobotA, com_Num=1)
        elif index == 3:
            val = arm.current_poseRobotA[2]
            arm.current_poseRobotA[2] = val - self.step
            arm.set_joint_angle(com_Pose=arm.current_poseRobotA, com_Num=1)
        elif index ==4:
            val = arm.current_poseRobotA[3]
            arm.current_poseRobotA[3] = val - self.step
            arm.set_joint_angle(com_Pose=arm.current_poseRobotA, com_Num=1)
        elif index == 5:
            val = arm.current_poseRobotA[4]
            arm.current_poseRobotA[4] = val - self.step
            arm.set_joint_angle(com_Pose=arm.current_poseRobotA, com_Num=1)
        elif index ==6:
            val = arm.current_poseRobotA[5]
            arm.current_poseRobotA[5] = val - self.step
            arm.set_joint_angle(com_Pose=arm.current_poseRobotA, com_Num=1)
        elif index ==7:
            val = arm.current_poseRobotA[0]
            arm.current_poseRobotA[0] = val + self.step
            arm.set_joint_angle(com_Pose=arm.current_poseRobotA, com_Num=1)
        elif index ==8:
            val = arm.current_poseRobotA[1]
            arm.current_poseRobotA[1] = val + self.step
            arm.set_joint_angle(com_Pose=arm.current_poseRobotA, com_Num=1)
        elif index ==9:
            val = arm.current_poseRobotA[2]
            arm.current_poseRobotA[2] = val + self.step
            arm.set_joint_angle(com_Pose=arm.current_poseRobotA, com_Num=1)
        elif index ==10:
            val = arm.current_poseRobotA[3]
            arm.current_poseRobotA[3] = val + self.step
            arm.set_joint_angle(com_Pose=arm.current_poseRobotA, com_Num=1)
        elif index ==11:
            val = arm.current_poseRobotA[4]
            arm.current_poseRobotA[4] = val + self.step
            arm.set_joint_angle(com_Pose=arm.current_poseRobotA, com_Num=1)
        elif index ==12:
            val = arm.current_poseRobotA[5]
            arm.current_poseRobotA[5] = val + self.step
            arm.set_joint_angle(com_Pose=arm.current_poseRobotA, com_Num=1)

    def jointControRobotA(self,index):
        try:
            speed = 2000
            s_data = server.vsrserver.robot_control.status_global
            angle_current = s_data["angle"]
            angle_current = [int(i) for i in angle_current]
            if self.robotA_type == "KT-R6":
                ### 1 2 3 4 5 6 轴角 减  7 8 9 10 11 12 轴角 加  21 复位  21 回零 23 急停
                if index == 1:
                    gcode = "M21 G90 G01 X-100 Y{} Z{} A{} B{} C{} F{}\n".format(angle_current[1],angle_current[2],\
                        angle_current[3],angle_current[4],angle_current[5], speed)
                elif index == 2:
                    gcode = "M21 G90 G01 X{} Y-35 Z{} A{} B{} C{} F{}\n".format(angle_current[0],angle_current[2],\
                        angle_current[3],angle_current[4],angle_current[5], speed)
                elif index == 3:
                    gcode = "M21 G90 G01 X{} Y{} Z-120 A{} B{} C{} F{}\n".format(angle_current[0],angle_current[1],\
                        angle_current[3],angle_current[4],angle_current[5], speed)
                elif index == 4:
                    gcode = "M21 G90 G01 X{} Y{} Z{} A-180 B{} C{} F{}\n".format(angle_current[0],angle_current[1],\
                        angle_current[2],angle_current[4],angle_current[5], speed)
                elif index == 5:
                    gcode = "M21 G90 G01 X{} Y{} Z{} A{} B-180 C{} F{}\n".format(angle_current[0],angle_current[1],\
                        angle_current[2],angle_current[3],angle_current[5], speed)
                elif index == 6:
                    gcode = "M21 G90 G01 X{} Y{} Z{} A{} B{} C-349 F{}\n".format(angle_current[0],angle_current[1],\
                        angle_current[2],angle_current[3],angle_current[4], speed)
                elif index == 7:
                    gcode = "M21 G90 G01 X160 Y{} Z{} A{} B{} C{} F{}\n".format(angle_current[1],angle_current[2],\
                        angle_current[3],angle_current[4],angle_current[5], speed)
                elif index == 8:
                    gcode = "M21 G90 G01 X{} Y70 Z{} A{} B{} C{} F{}\n".format(angle_current[0],angle_current[2],\
                        angle_current[3],angle_current[4],angle_current[5], speed)
                elif index == 9:
                    gcode = "M21 G90 G01 X{} Y{} Z60 A{} B{} C{} F{}\n".format(angle_current[0],angle_current[1],\
                        angle_current[3],angle_current[4],angle_current[5], speed)
                elif index == 10:
                    gcode = "M21 G90 G01 X{} Y{} Z{} A140 B{} C{} F{}\n".format(angle_current[0],angle_current[1],\
                        angle_current[2],angle_current[4],angle_current[5], speed)
                elif index == 11:
                    gcode = "M21 G90 G01 X{} Y{} Z{} A{} B29 C{} F{}\n".format(angle_current[0],angle_current[1],\
                        angle_current[2],angle_current[3],angle_current[5], speed)
                elif index == 12:
                    gcode = "M21 G90 G01 X{} Y{} Z{} A{} B{} C349 F{}\n".format(angle_current[0],angle_current[1],\
                        angle_current[2],angle_current[3],angle_current[4], speed)

            elif self.robotA_type == "KT-R4":
                ### 1 2 3 4 5 6 轴角 减  7 8 9 10 11 12 轴角 加  21 复位  21 回零 23 急停
                if index == 1:
                    gcode = "M21 G90 G01 X-130 Y{} Z{} A{} B{} C{} F{}\n".format(angle_current[1],angle_current[2],\
                        angle_current[3],angle_current[4],angle_current[5], speed)
                elif index == 2:
                    gcode = "M21 G90 G01 X{} Y-35 Z{} A{} B{} C{} F{}\n".format(angle_current[0],angle_current[2],\
                        angle_current[3],angle_current[4],angle_current[5], speed)
                elif index == 3:
                    gcode = "M21 G90 G01 X{} Y{} Z-10 A{} B{} C{} F{}\n".format(angle_current[0],angle_current[1],\
                        angle_current[3],angle_current[4],angle_current[5], speed)
                elif index == 4:
                    gcode = "M21 G90 G01 X{} Y{} Z{} A-180 B{} C{} F{}\n".format(angle_current[0],angle_current[1],\
                        angle_current[2],angle_current[4],angle_current[5], speed)
                elif index == 7:
                    gcode = "M21 G90 G01 X180 Y{} Z{} A{} B{} C{} F{}\n".format(angle_current[1],angle_current[2],\
                        angle_current[3],angle_current[4],angle_current[5], speed)
                elif index == 8:
                    gcode = "M21 G90 G01 X{} Y45 Z{} A{} B{} C{} F{}\n".format(angle_current[0],angle_current[2],\
                        angle_current[3],angle_current[4],angle_current[5], speed)
                elif index == 9:
                    gcode = "M21 G90 G01 X{} Y{} Z40 A{} B{} C{} F{}\n".format(angle_current[0],angle_current[1],\
                        angle_current[3],angle_current[4],angle_current[5], speed)
                elif index == 10:
                    gcode = "M21 G90 G01 X{} Y{} Z{} A180 B{} C{} F{}\n".format(angle_current[0],angle_current[1],\
                        angle_current[2],angle_current[4],angle_current[5], speed)
                
            server.vsrserver.robot_control.send_msg(gcode, wait_idle= False, com_flag = 1)
            

        except Exception as e:
            robot_logger.error(e,exc_info=True)

    def jointControRobotRelease(self,index):
        try:
            arm.robot_stop()
            # gcode = "o117\r\n"
            # gcode = "!\r\n"
            # server.vsrserver.robot_control.send_msg(gcode, wait_idle= False, com_flag = 1)
            # time.sleep(0.5)
            # gcode = f"%\n"
            # server.vsrserver.robot_control.send_msg(gcode, wait_idle= False, com_flag = 1)
        
        except Exception as e:
            robot_logger.error(e,exc_info=True)

    def robotA_coordinateTeach(self):
        try:
            coordinate_PushbuttonA = {
                self.pushButton_C1min: 1,
                self.pushButton_C2min: 2,
                self.pushButton_C3min: 3,
                self.pushButton_C4min: 4,
                self.pushButton_C5min: 5,
                self.pushButton_C6min: 6,
                self.pushButton_C1add: 7,
                self.pushButton_C2add: 8,
                self.pushButton_C3add: 9,
                self.pushButton_C4add: 10,
                self.pushButton_C5add: 11,
                self.pushButton_C6add: 12,

            }

            for butt, page_ind in coordinate_PushbuttonA.items():
                # butt.clicked.connect(lambda checked, index=page_ind: self.coordControRobotA(index))
                # 连接 pressed 信号到第一个函数
                butt.pressed.connect(lambda checked=False, index=page_ind: self._coordControRobotA(index))
                # 连接 released 信号到第二个函数
                butt.released.connect(lambda checked=False, index=page_ind: self.coordControRobotRelease(index))
        
        except Exception as e:
            robot_logger.error(e,exc_info=True)

    ## 虚拟控制器
    def _coordControRobotA(self, index):
        self.step = 5.0
        if index == 1:
            val = arm.current_poseRobotA[6]
            arm.current_poseRobotA[6] = val - self.step
            arm.set_joint_angle(com_Pose=arm.current_poseRobotA, com_Num=1)
        elif index ==2:
            val = arm.current_poseRobotA[7]
            arm.current_poseRobotA[7] = val - self.step
            arm.set_joint_angle(com_Pose=arm.current_poseRobotA, com_Num=1)
        elif index == 3:
            val = arm.current_poseRobotA[8]
            arm.current_poseRobotA[8] = val - self.step
            arm.set_joint_angle(com_Pose=arm.current_poseRobotA, com_Num=1)
        elif index ==4:
            val = arm.current_poseRobotA[9]
            arm.current_poseRobotA[9] = val - self.step
            arm.set_joint_angle(com_Pose=arm.current_poseRobotA, com_Num=1)
        elif index == 5:
            val = arm.current_poseRobotA[10]
            arm.current_poseRobotA[10] = val - self.step
            arm.set_joint_angle(com_Pose=arm.current_poseRobotA, com_Num=1)
        elif index ==6:
            val = arm.current_poseRobotA[11]
            arm.current_poseRobotA[11] = val - self.step
            arm.set_joint_angle(com_Pose=arm.current_poseRobotA, com_Num=1)
        elif index ==7:
            val = arm.current_poseRobotA[6]
            arm.current_poseRobotA[6] = val + self.step
            arm.set_joint_angle(com_Pose=arm.current_poseRobotA, com_Num=1)
        elif index ==8:
            val = arm.current_poseRobotA[7]
            arm.current_poseRobotA[7] = val + self.step
            arm.set_joint_angle(com_Pose=arm.current_poseRobotA, com_Num=1)
        elif index ==9:
            val = arm.current_poseRobotA[8]
            arm.current_poseRobotA[8] = val + self.step
            arm.set_joint_angle(com_Pose=arm.current_poseRobotA, com_Num=1)
        elif index ==10:
            val = arm.current_poseRobotA[9]
            arm.current_poseRobotA[9] = val + self.step
            arm.set_joint_angle(com_Pose=arm.current_poseRobotA, com_Num=1)
        elif index ==11:
            val = arm.current_poseRobotA[10]
            arm.current_poseRobotA[10] = val + self.step
            arm.set_joint_angle(com_Pose=arm.current_poseRobotA, com_Num=1)
        elif index ==12:
            val = arm.current_poseRobotA[11]
            arm.current_poseRobotA[11] = val + self.step
            arm.set_joint_angle(com_Pose=arm.current_poseRobotA, com_Num=1)

    def coordControRobotA(self,index):
        try:
            speed = self.speed
            s_data = server.vsrserver.robot_control.status_global
            angle_current = s_data["angle"]
            angle_current = [int(i) for i in angle_current]
            ### 1 2 3 4 5 6 笛卡尔 减  7 8 9 10 11 12 笛卡尔 加
            if index == 1:
                gcode = f"M20 G91 G01 X-{self.step} Y0 Z0 A0 B0 C00 F{speed}\n"
            elif index == 2:
                gcode = f"M20 G91 G01 X0 Y-{self.step} Z0 A0 B0 C00 F{speed}\n"
            elif index == 3:
                gcode = f"M20 G91 G01 X0 Y0 Z-{self.step} A0 B0 C00 F{speed}\n"
            elif index == 4:
                gcode = f"M20 G91 G01 X0 Y0 Z0 A-{self.step} B0 C00 F{speed}\n"
            elif index == 5:
                gcode = f"M20 G91 G01 X0 Y0 Z0 A0 B-{self.step} C00 F{speed}\n"
            elif index == 6:
                gcode = f"M20 G91 G01 X0 Y0 Z0 A0 B0 C-{self.step} F{speed}\n"
            elif index == 7:
                gcode = f"M20 G91 G01 X{self.step} Y0 Z0 A0 B0 C00 F{speed}\n"
            elif index == 8:
                gcode = f"M20 G91 G01 X0 Y{self.step} Z0 A0 B0 C00 F{speed}\n"
            elif index == 9:
                gcode = f"M20 G91 G01 X0 Y0 Z{self.step} A0 B0 C00 F{speed}\n"
            elif index == 10:
                gcode = f"M20 G91 G01 X0 Y0 Z0 A{self.step} B0 C00 F{speed}\n"
            elif index == 11:
                gcode = f"M20 G91 G01 X0 Y0 Z0 A0 B{self.step} C00 F{speed}\n"
            elif index == 12:
                gcode = f"M20 G91 G01 X0 Y0 Z0 A0 B0 C{self.step} F{speed}\n"            
            
            server.vsrserver.robot_control.send_msg(gcode, wait_idle= False, com_flag = 1)

        except Exception as e:
            robot_logger.error(e,exc_info=True)

    def coordControRobotRelease(self,index):
        try:
            arm.robot_stop()
            # # gcode = "o117\r\n"
            # gcode = "!\r\n"
            # server.vsrserver.robot_control.send_msg(gcode, wait_idle= False, com_flag = 1)
            # time.sleep(0.5)
            # gcode = f"%\n"
            # server.vsrserver.robot_control.send_msg(gcode, wait_idle= False, com_flag = 1)

        except Exception as e:
            robot_logger.error(e,exc_info=True)

    ###-- 6. B机器人控制：robotB_jointTeach轴角   robotB_coordinateTeach笛卡尔示教
    def robotB_jointTeach(self):
        try:
            joint_PushbuttonB = {
                self.pushButton_J1min_B: 1,
                self.pushButton_J2min_B: 2,
                self.pushButton_J3min_B: 3,
                self.pushButton_J4min_B: 4,
                self.pushButton_J5min_B: 5,
                self.pushButton_J6min_B: 6,
                self.pushButton_J1add_B: 7,
                self.pushButton_J2add_B: 8,
                self.pushButton_J3add_B: 9,
                self.pushButton_J4add_B: 10,
                self.pushButton_J5add_B: 11,
                self.pushButton_J6add_B: 12,
            }
            for butt, page_ind in joint_PushbuttonB.items():
                butt.clicked.connect(lambda checked, index=page_ind: self._jointControRobotB(index))
        
        except Exception as e:
            robot_logger.error(e,exc_info=True)

    def _jointControRobotB(self,index):
        self.step = 5.0
        if index == 1:
            val = arm.current_poseRobotB[0]
            arm.current_poseRobotB[0] = val - self.step
            arm.set_joint_angle(com_Pose=arm.current_poseRobotB, com_Num=2)
        elif index ==2:
            val = arm.current_poseRobotB[1]
            arm.current_poseRobotB[1] = val - self.step
            arm.set_joint_angle(com_Pose=arm.current_poseRobotB, com_Num=2)
        elif index == 3:
            val = arm.current_poseRobotB[2]
            arm.current_poseRobotB[2] = val - self.step
            arm.set_joint_angle(com_Pose=arm.current_poseRobotB, com_Num=2)
        elif index ==4:
            val = arm.current_poseRobotB[3]
            arm.current_poseRobotB[3] = val - self.step
            arm.set_joint_angle(com_Pose=arm.current_poseRobotB, com_Num=2)
        elif index == 5:
            val = arm.current_poseRobotB[4]
            arm.current_poseRobotB[4] = val - self.step
            arm.set_joint_angle(com_Pose=arm.current_poseRobotB, com_Num=2)
        elif index ==6:
            val = arm.current_poseRobotB[5]
            arm.current_poseRobotB[5] = val - self.step
            arm.set_joint_angle(com_Pose=arm.current_poseRobotB, com_Num=2)
        elif index ==7:
            val = arm.current_poseRobotB[0]
            arm.current_poseRobotB[0] = val + self.step
            arm.set_joint_angle(com_Pose=arm.current_poseRobotB, com_Num=2)
        elif index ==8:
            val = arm.current_poseRobotB[1]
            arm.current_poseRobotB[1] = val + self.step
            arm.set_joint_angle(com_Pose=arm.current_poseRobotB, com_Num=2)
        elif index ==9:
            val = arm.current_poseRobotB[2]
            arm.current_poseRobotB[2] = val + self.step
            arm.set_joint_angle(com_Pose=arm.current_poseRobotB, com_Num=2)
        elif index ==10:
            val = arm.current_poseRobotB[3]
            arm.current_poseRobotB[3] = val + self.step
            arm.set_joint_angle(com_Pose=arm.current_poseRobotB, com_Num=2)
        elif index ==11:
            val = arm.current_poseRobotB[4]
            arm.current_poseRobotB[4] = val + self.step
            arm.set_joint_angle(com_Pose=arm.current_poseRobotB, com_Num=2)
        elif index ==12:
            val = arm.current_poseRobotB[5]
            arm.current_poseRobotB[5] = val + self.step
            arm.set_joint_angle(com_Pose=arm.current_poseRobotB, com_Num=2)

    def jointControRobotB(self,index):
        try:
            speed = 2000
            s_data_ = server.vsrserver.robot_control.status_global_
            angle_current = s_data_["angle"]
            angle_current = [int(i) for i in angle_current]
            ### 1 2 3 4 5 6 轴角 减  7 8 9 10 11 12 轴角 加
            if index == 1:
                gcode = "M21 G90 G01 X-100 Y{} Z{} A{} B{} C{} F{}\n".format(angle_current[1],angle_current[2],\
                    angle_current[3],angle_current[4],angle_current[5], speed)
            elif index == 2:
                gcode = "M21 G90 G01 X{} Y-35 Z{} A{} B{} C{} F{}\n".format(angle_current[0],angle_current[2],\
                    angle_current[3],angle_current[4],angle_current[5], speed)
            elif index == 3:
                gcode = "M21 G90 G01 X{} Y{} Z-120 A{} B{} C{} F{}\n".format(angle_current[0],angle_current[1],\
                    angle_current[3],angle_current[4],angle_current[5], speed)
            elif index == 4:
                gcode = "M21 G90 G01 X{} Y{} Z{} A-180 B{} C{} F{}\n".format(angle_current[0],angle_current[1],\
                    angle_current[2],angle_current[4],angle_current[5], speed)
            elif index == 5:
                gcode = "M21 G90 G01 X{} Y{} Z{} A{} B-180 C{} F{}\n".format(angle_current[0],angle_current[1],\
                    angle_current[2],angle_current[3],angle_current[5], speed)
            elif index == 6:
                gcode = "M21 G90 G01 X{} Y{} Z{} A{} B{} C-349 F{}\n".format(angle_current[0],angle_current[1],\
                    angle_current[2],angle_current[3],angle_current[4], speed)
            elif index == 7:
                gcode = "M21 G90 G01 X100 Y{} Z{} A{} B{} C{} F{}\n".format(angle_current[1],angle_current[2],\
                    angle_current[3],angle_current[4],angle_current[5], speed)
            elif index == 8:
                gcode = "M21 G90 G01 X{} Y70 Z{} A{} B{} C{} F{}\n".format(angle_current[0],angle_current[2],\
                    angle_current[3],angle_current[4],angle_current[5], speed)
            elif index == 9:
                gcode = "M21 G90 G01 X{} Y{} Z120 A{} B{} C{} F{}\n".format(angle_current[0],angle_current[1],\
                    angle_current[3],angle_current[4],angle_current[5], speed)
            elif index == 10:
                gcode = "M21 G90 G01 X{} Y{} Z{} A180 B{} C{} F{}\n".format(angle_current[0],angle_current[1],\
                    angle_current[2],angle_current[4],angle_current[5], speed)
            elif index == 11:
                gcode = "M21 G90 G01 X{} Y{} Z{} A{} B29 C{} F{}\n".format(angle_current[0],angle_current[1],\
                    angle_current[2],angle_current[3],angle_current[5], speed)
            elif index == 12:
                gcode = "M21 G90 G01 X{} Y{} Z{} A{} B{} C349 F{}\n".format(angle_current[0],angle_current[1],\
                    angle_current[2],angle_current[3],angle_current[4], speed)

            server.vsrserver.robot_control.send_msg(gcode, wait_idle= False, com_flag = 2)
            

        except Exception as e:
            robot_logger.error(e,exc_info=True)

    def robotB_coordinateTeach(self):
        try:
            coordinate_PushbuttonB = {
                self.pushButton_C1min_B: 1,
                self.pushButton_C2min_B: 2,
                self.pushButton_C3min_B: 3,
                self.pushButton_C4min_B: 4,
                self.pushButton_C5min_B: 5,
                self.pushButton_C6min_B: 6,
                self.pushButton_C1add_B: 7,
                self.pushButton_C2add_B: 8,
                self.pushButton_C3add_B: 9,
                self.pushButton_C4add_B: 10,
                self.pushButton_C5add_B: 11,
                self.pushButton_C6add_B: 12,
            }

            for butt, page_ind in coordinate_PushbuttonB.items():
                butt.clicked.connect(lambda checked, index=page_ind: self.coordControRobotB(index))
        
        except Exception as e:
            robot_logger.error(e,exc_info=True)

    def _coordControRobotB(self,index):
        self.step = 5.0
        if index == 1:
            val = arm.current_poseRobotB[6]
            arm.current_poseRobotB[6] = val - self.step
            arm.set_joint_angle(com_Pose=arm.current_poseRobotB, com_Num=2)
        elif index ==2:
            val = arm.current_poseRobotB[7]
            arm.current_poseRobotB[7] = val - self.step
            arm.set_joint_angle(com_Pose=arm.current_poseRobotB, com_Num=2)
        elif index == 3:
            val = arm.current_poseRobotB[8]
            arm.current_poseRobotB[8] = val - self.step
            arm.set_joint_angle(com_Pose=arm.current_poseRobotB, com_Num=2)
        elif index ==4:
            val = arm.current_poseRobotB[9]
            arm.current_poseRobotB[9] = val - self.step
            arm.set_joint_angle(com_Pose=arm.current_poseRobotB, com_Num=2)
        elif index == 5:
            val = arm.current_poseRobotB[10]
            arm.current_poseRobotB[10] = val - self.step
            arm.set_joint_angle(com_Pose=arm.current_poseRobotB, com_Num=2)
        elif index ==6:
            val = arm.current_poseRobotB[11]
            arm.current_poseRobotB[11] = val - self.step
            arm.set_joint_angle(com_Pose=arm.current_poseRobotB, com_Num=2)
        elif index ==7:
            val = arm.current_poseRobotB[6]
            arm.current_poseRobotB[6] = val + self.step
            arm.set_joint_angle(com_Pose=arm.current_poseRobotB, com_Num=2)
        elif index ==8:
            val = arm.current_poseRobotB[7]
            arm.current_poseRobotB[7] = val + self.step
            arm.set_joint_angle(com_Pose=arm.current_poseRobotB, com_Num=2)
        elif index ==9:
            val = arm.current_poseRobotB[8]
            arm.current_poseRobotB[8] = val + self.step
            arm.set_joint_angle(com_Pose=arm.current_poseRobotB, com_Num=2)
        elif index ==10:
            val = arm.current_poseRobotB[9]
            arm.current_poseRobotB[9] = val + self.step
            arm.set_joint_angle(com_Pose=arm.current_poseRobotB, com_Num=2)
        elif index ==11:
            val = arm.current_poseRobotB[10]
            arm.current_poseRobotB[10] = val + self.step
            arm.set_joint_angle(com_Pose=arm.current_poseRobotB, com_Num=2)
        elif index ==12:
            val = arm.current_poseRobotB[11]
            arm.current_poseRobotB[11] = val + self.step
            arm.set_joint_angle(com_Pose=arm.current_poseRobotB, com_Num=2)

    def coordControRobotB(self,index):
        try:
            speed = self.speed
            s_data_ = server.vsrserver.robot_control.status_global_
            angle_current = s_data_["angle"]
            angle_current = [int(i) for i in angle_current]
            ### 1 2 3 4 5 6 笛卡尔 减  7 8 9 10 11 12 笛卡尔 加
            if index == 1:
                gcode = f"M20 G91 G01 X-{self.step} Y0 Z0 A0 B0 C00 F{speed}\n"
            elif index == 2:
                gcode = f"M20 G91 G01 X0 Y-{self.step} Z0 A0 B0 C00 F{speed}\n"
            elif index == 3:
                gcode = f"M20 G91 G01 X0 Y0 Z-{self.step} A0 B0 C00 F{speed}\n"
            elif index == 4:
                gcode = f"M20 G91 G01 X0 Y0 Z0 A-{self.step} B0 C00 F{speed}\n"
            elif index == 5:
                gcode = f"M20 G91 G01 X0 Y0 Z0 A0 B-{self.step} C00 F{speed}\n"
            elif index == 6:
                gcode = f"M20 G91 G01 X0 Y0 Z0 A0 B0 C-{self.step} F{speed}\n"
            elif index == 7:
                gcode = f"M20 G91 G01 X{self.step} Y0 Z0 A0 B0 C00 F{speed}\n"
            elif index == 8:
                gcode = f"M20 G91 G01 X0 Y{self.step} Z0 A0 B0 C00 F{speed}\n"
            elif index == 9:
                gcode = f"M20 G91 G01 X0 Y0 Z{self.step} A0 B0 C00 F{speed}\n"
            elif index == 10:
                gcode = f"M20 G91 G01 X0 Y0 Z0 A{self.step} B0 C00 F{speed}\n"
            elif index == 11:
                gcode = f"M20 G91 G01 X0 Y0 Z0 A0 B{self.step} C00 F{speed}\n"
            elif index == 12:
                gcode = f"M20 G91 G01 X0 Y0 Z0 A0 B0 C{self.step} F{speed}\n"            
            
            server.vsrserver.robot_control.send_msg(gcode, wait_idle= False, com_flag = 2)

        except Exception as e:
            robot_logger.error(e,exc_info=True)

    ###-- 7. 复位 急停 回零（命令机械臂）
    def cmd_robot(self):
        try:
            cmd_pushButton = {
                self.pushButton_robotHome:21,
                self.pushButton_robotZero:22,
                self.pushButton_robotStop:23,
            }

            cmd_pushButtonB = {
                self.pushButton_robotHome_B:31,
                self.pushButton_robotZero_B:32,
                self.pushButton_robotStop_B:33,
            }
            for butt, page_ind in cmd_pushButton.items():
                butt.clicked.connect(lambda checked, index=page_ind: self.cmd_robotControl(index))
            
            for butt, page_ind in cmd_pushButtonB.items():
                butt.clicked.connect(lambda checked, index=page_ind: self.cmd_robotControlB(index))
        except Exception as e:
            robot_logger.error(e,exc_info=True)

    def cmd_robotControl(self,index):
        try:
                if index == 21:
                    gcode = "$h\r\n"
                elif index == 22:
                    gcode = 'M21 G90 G00 X0 Y0 Z0 A0 B0 C0 F2000\r\n'
                elif index == 23:
                    gcode = "o117\r\n"

                server.vsrserver.robot_control.send_msg(gcode, wait_idle= False, com_flag = 1)

        except Exception as e:
            robot_logger.error(e,exc_info=True)
    
    def cmd_robotControlB(self,index):
        try:
                if index == 31:
                    gcode = "$h\r\n"
                elif index == 32:
                    gcode = 'M21 G90 G00 X0 Y0 Z0 A0 B0 C0 F2000\r\n'
                elif index == 33:
                    gcode = "o117\r\n"

                server.vsrserver.robot_control.send_msg(gcode, wait_idle= False, com_flag = 2)

        except Exception as e:
            robot_logger.error(e,exc_info=True)


####### 手动控制界面------End


##############################
####### 状态监控界面------Start

    #### 0. 按钮初始化
    def mointor_pushButton(self):
        try:
            self.textBrowser_modbusClient.setMaximumHeight(60)  # 设定固定的打印高度
            self.textBrowser_modbusClient.setVerticalScrollBarPolicy(Qt.ScrollBarAlwaysOff)  # 禁用垂直滚动条
            self.pushButton_modbusClientClear.clicked.connect(self.clearModbusClients)

            buttons = {
                self.pushButton_bool: 0,
                self.pushButton_int: 1,
                self.pushButton_float: 2,
                self.pushButton_pose: 3,
                self.pushButton_io:4,
            }
            for button, page_index in buttons.items():
                ### 设置默认样式
                default_style = "QPushButton { background-color: white; }"
                active_style = "QPushButton { background-color: lightblue; }"

                button.setStyleSheet(default_style)

                def on_click(checked, index=page_index, btn=button):
                    ### 当按钮被点击时，改变当前索引并更新样式
                    self.stackedWidget_mointor.setCurrentIndex(index)
                    ### 移除所有按钮的激活样式
                    for b in buttons:
                        b.setStyleSheet(default_style)
                    ### 给被点击的按钮应用激活样式
                    btn.setStyleSheet(active_style)

                ### 连接按钮点击信号到槽函数
                button.clicked.connect(on_click)
        
        except Exception as e:
            robot_logger.error(e,exc_info=True)

    ###-- 1-4. IO型变量监控
    def _initIOValuesMonitor(self):
        try:
            for i in range (0,4):
                do_is_open = master.get_All_DO(i)
                # do_is_open = master.get_DO(i)
                # label_name = f'DO{i}'
                label_name = f'label_light_do{i}_2'
                label = getattr(self, label_name, None)
                if label is None:
                    print(f"Warning: {label_name} not found.")
                    continue
                if do_is_open:
                    label.setStyleSheet("background-color: green; border-radius: 10px;")
                else:
                    label.setStyleSheet("background-color: white; border-radius: 10px;")
            ## 虚拟控制器注释
            ## 这里得修改ui中的DI按钮
            # for i in range (0,12):
            #     # di_is_open = process_master.get_All_DI(i)
            #     di_is_open = master.get_DI(i)
            #     label_di_name = f'label_light_di{i}_2'
            #     label_di = getattr(self, label_di_name, None)
            #     if label_di is None:
            #         print(f"Warning: {label_di_name} not found.")
            #         continue
            #     if di_is_open:
            #         label_di.setStyleSheet("background-color: green; border-radius: 10px;")
            #     else:
            #         label_di.setStyleSheet("background-color: white; border-radius: 10px;")

            # # motor = process_master.get_Motor(0)
            # for i in range(2):
            #     motor = master.get_Motor(i)
            #     flag = 3 - i
            #     self.motor_color(flag, motor)

        except Exception as e:
            print(f"Error: {str(e)}")
    
    def clearModbusClients(self):
        self.msgVisonRes_history.clear()
        self.textBrowser_modbusClient.clear()

    ### 1. 初始化按钮、视图等信息
    def dataPageInit(self):
        self.table_views = {
            'bool': [self.tableView_bool1_1, self.tableView_bool1_2, self.tableView_bool1_3,
                    self.tableView_bool2_1, self.tableView_bool2_2, self.tableView_bool2_3],
            'int': [self.tableView_int1_1, self.tableView_int1_2, self.tableView_int1_3,
                    self.tableView_int2_1, self.tableView_int2_2, self.tableView_int2_3],
            'float': [self.tableView_float1_1, self.tableView_float1_2, self.tableView_float1_3,
                    self.tableView_float2_1, self.tableView_float2_2, self.tableView_float2_3],
            'pose': [self.tableView_pose1_1, self.tableView_pose1_2, self.tableView_pose1_3,
                    self.tableView_pose2_1, self.tableView_pose2_2, self.tableView_pose2_3]
        }

        self.page_buttons = {
            'bool': (self.pushButton_bool_pageup, self.pushButton_bool_pagedown, self.stackedWidget_boolDataPage),
            'int': (self.pushButton_int_pageup, self.pushButton_int_pagedown, self.stackedWidget_intDataPage),
            'float': (self.pushButton_float_pageup, self.pushButton_float_pagedown, self.stackedWidget_floatDataPage),
            'pose': (self.pushButton_pose_pageup, self.pushButton_pose_pagedown, self.stackedWidget_poseDataPage)
        }

        self.data_generators = {
            'bool': self.create_bool_table_data,
            'int': self.create_int_table_data,
            'float': self.create_float_table_data,
            'pose': self.create_pose_table_data
        }
    
    ### 1.2 设置成功页面
    def setup_pages(self, data_type):
        try:
            # self._timerUpdateModbusValue_new()
            page_up, page_down, stacked_widget = self.page_buttons[data_type]

            # Connect the page buttons to change the current index of the stacked widget
            for butt, index in zip([page_up, page_down], [0, 1]):
                butt.clicked.connect(lambda checked, idx=index: stacked_widget.setCurrentIndex(idx))

            # Setup table views with corresponding data
            start, step = 1, 7
            for table_view in self.table_views[data_type]:
                self.setup_table_view(table_view, self.data_generators[data_type], start, start + step - 1)
                start += step
        
        except Exception as e:
            print(f"setup_pages error is {e}")

    ### 1.2.1 创建bool视图表格
    def create_bool_table_data(self, start, end):
        return [[str(i), f'GB{i:02d}', f'0X_{(i-1):04d}', 'False'] for i in range(start, end+1)]
    
    ### 1.2.2 创建int视图表格
    def create_int_table_data(self, start, end):
        return [[str(i), f'GI{i:02d}', f'4X_{(300 + (i-1)):04d}', '0'] for i in range(start, end+1)]

    ### 1.2.3 创建float视图表格
    def create_float_table_data(self, start, end):
        return [[str(i), f'GF{i:02d}', f'4X_{(400 + (i-1)*2):04d}', '0.0'] for i in range(start, end+1)]
    
    ### 1.2.4 创建pose视图表格
    def create_pose_table_data(self, start, end):
        return [[str(i), f'GP{i:02d}', f'4X_{(500 + (i-1)*2):04d}', '00.00'] for i in range(start, end+1)]

    ### 1.3 初始化填充视图表格，单元格格式等
    def setup_table_view(self, table_view, data_generator, start, end):
        try:
            """设置指定范围的tableView"""
            table_data = data_generator(start, end)
            model = MyTableModel(table_data)
            table_view.setModel(model)

            horizontal_header = table_view.horizontalHeader()
            horizontal_header.setSectionResizeMode(0, QHeaderView.Fixed)
            horizontal_header.resizeSection(0, 46)
            horizontal_header.resizeSection(1, 100)
            horizontal_header.resizeSection(2, 160)
            horizontal_header.resizeSection(3, 76)

            ### 自适应列宽
            # for i in range(1, horizontal_header.count()):
            #     horizontal_header.setSectionResizeMode(i, QHeaderView.Stretch)

            font = QFont("Arial", 18, QFont.Normal)
            table_view.setFont(font)

            header_font = QFont("Arial", 18, QFont.Bold)
            horizontal_header.setFont(header_font)
            horizontal_header.setStyleSheet("::section{height:70px;}")

            QTimer.singleShot(0, lambda tv=table_view: adjust_row_heights(tv))
            table_view.show()
        
        except Exception as e:
            print(f"setup_table_view error is {e}")

    ### 1.4 设置定时器以定期更新值(500ms/次)

    ### 1.5 变量数据监控
    def valueMonitor(self):
        modbus_addr_prefix = ["0X", "4X"]
        self.update_data_from_modbus(modbus_addr_prefix)    

    #### 1.6 """从Modbus设备更新数据"""
    def update_data_from_modbus(self, prefixes):
        data = self.read_modbus_register(prefixes)
        for table_type in ['bool', 'int', 'float', 'pose']:
            self.update_table_view_data(table_type, data)

    ### 1.7 """根据类型更新tableView的数据"""
    def update_table_view_data(self, data_type, data):
        start, step = 1, 7
        for table_view in self.table_views[data_type]:
            model = table_view.model()
            if model is not None:
                for row in range(model.rowCount()):
                    index = model.index(row, 3)  # 第4列索引
                    register_address = self.get_modbus_address(data_type, start + row - 1)
                    
                    # # 对于 float 和 pose 类型，确保只使用偶数地址作为键
                    # if data_type in ['float', 'pose']:
                    #     register_address = register_address if register_address % 2 == 0 else register_address - 1

                    prefix = "4X" if data_type in ['int', 'float', 'pose'] else "0X"
                    key = f"{prefix}_{register_address:04d}"  # 确保地址有四位数

                    if key in data:
                        value = data[key]
                        
                        # 根据值类型格式化显示
                        if isinstance(value, bool):
                            display_value = "False" if not value else "True"
                        elif isinstance(value, int):
                            display_value = str(value)
                        elif isinstance(value, float):
                            display_value = f"{value:.2f}"
                        else:
                            display_value = str(value)

                        model.setData(index, display_value, Qt.EditRole)
                        ## 尝试设置数据并检查是否成功 
                        # if model.setData(index, display_value, Qt.EditRole):
                        #     print(f"Data successfully set for index {index} to {display_value}")
                        # else:
                        #     print(f"Failed to set data for index {index}")
                    
                    # else:
                    #     print(f"Key {key} not found in data.")
                
                # 强制刷新视图
                model.layoutAboutToBeChanged.emit()
                model.layoutChanged.emit()
                table_view.viewport().update()
            
            start += step

    ### 1.8 读取寄存器地址的值
    def read_modbus_register(self, prefixes):
        try:
            data = {}
            for prefix in prefixes:
                if prefix == "0X":
                    pass
                    # 读取线圈（布尔值）
                    # response = process_master.master_.execute(1, cst.READ_COILS, starting_address=0, quantity_of_x=42)
                    # data.update({f"{prefix}_{i:04d}": bool(response[i]) for i in range(len(response))})
                    response = [] 
                    for i in range(42):
                        value = master.get_coils_register( i, 1)[0]
                        response.append(value)
                    data.update({f"{prefix}_{i:04d}": bool(response[i]) for i in range(len(response))})
                elif prefix == "4X":
                    value_tmp = {
                        'int': 300,
                        'float': 400,
                        'pose': 500
                    }
                    quantities = {
                        'int': 42,
                        'float': 82,
                        'pose': 82
                    }

                    for var_type, start_addr in value_tmp.items():
                        # response = process_master.master_.execute(1, cst.READ_HOLDING_REGISTERS, starting_address=start_addr, quantity_of_x=quantities[var_type])
                        response = master.get_hold_register( start_addr, quantities[var_type])
                        if var_type == 'int':
                            data.update({f"{prefix}_{start_addr + i:04d}": response[i] for i in range(len(response))})
                        elif var_type in ['float', 'pose']:
                            for i in range(0, len(response), 2):  # 浮点数和位姿占用两个寄存器
                                high_word = response[i]
                                low_word = response[i + 1]
                                float_value = struct.unpack('>f', struct.pack('>HH', high_word, low_word))[0]
                                data[f"{prefix}_{start_addr + i:04d}"] = float_value
            return data
        
        except Exception as e:
            print(f"read_modbus_register error is {e}")

    ### 1.9 """根据类型和索引计算Modbus寄存器地址"""
    def get_modbus_address(self, data_type, register_index):
        """根据类型和索引计算Modbus寄存器地址"""
        base_address = {
            'bool': 0,
            'int': 300,
            'float': 400,
            'pose': 500
        }
        
        # 定义每种类型的寄存器数量
        register_count = {
            'bool': 42,
            'int': 42,
            'float': 42,  # 注意：这是浮点数的数量，不是寄存器数量
            'pose': 42    # 同上
        }

        if register_index > register_count[data_type]:
            raise ValueError(f"Register index {register_index} out of range for {data_type}")

        address_offset = register_index
        if data_type in ['float', 'pose']:
            address_offset *= 2  # 因为浮点数和位姿占用两个寄存器
        
        return base_address[data_type] + address_offset

        # def get_modbus_address(self, data_type, register_index):

        #     base_address = {
        #         'bool': 0,
        #         'int': 300,
        #         'float': 400,
        #         'pose': 500
        #     }
        #     address_offset = register_index - 1
        #     if data_type in ['float', 'pose']:
        #         address_offset *= 2  # 因为浮点数和位姿占用两个寄存器
        #     return base_address[data_type] + address_offset

    ####### 状态监控界面------End


##############################
####### 机器视觉界面------Start
    ### 1. 视觉按钮初始化信号与槽
    def vision_initSingal(self):
        self.textBrowser_visionParam.setMaximumHeight(504)  # 设定固定的打印高度
        self.textBrowser_visionParam.setVerticalScrollBarPolicy(Qt.ScrollBarAlwaysOff)  # 禁用垂直滚动条
        self.textBrowser_visionResultInfo.setMaximumHeight(72)  
        self.textBrowser_visionResultInfo.setVerticalScrollBarPolicy(Qt.ScrollBarAlwaysOff) 

        self.pushButton_visionProcess.clicked.connect(self.trigger_qr_detection)
        self.pushButton_visionCapture.clicked.connect(self.trigger_capture)
        self.pushButton_visionAutoManu.clicked.connect(self.autoUpdate)
        self.pushButton_clearPatameter.clicked.connect(self.clearVisionParameter)
        self.pushButton_clearResult.clicked.connect(self.clearVisionResult)

    """触发二维码识别"""
    def trigger_qr_detection(self):
        self.camera_thread.detect_qr_code = 1
        time.sleep(0.5)
        self.visionUpdateFrame()
    
    """停止二维码识别"""
    def stop_qr_detection(self):
        self.camera_thread.detect_qr_code = 0

    def trigger_capture(self):
        self.pauseUpdate()
        self.camera_thread.stop = False
        time.sleep(0.5)
        self.camera_thread.stop = True
        time.sleep(0.5)
        self.visionUpdateFrame(result_flag=False)

    ## 视觉显示--定时器用于定期检查队列中的新图像(不停刷新--本应用中有拍照，手动模式为拍一张)
    def autoUpdate(self):
        self.camera_thread.stop = False
        if not self.cameraUpdate_timer.isActive():
            self.cameraUpdate_timer.start(50)
    
    ### 暂停定时器
    def pauseUpdate(self):
        self.camera_thread.stop = True
        if self.cameraUpdate_timer.isActive():
            self.cameraUpdate_timer.stop()
    
    ### 恢复定时器
    def resumeUpdate(self):
        if not self.cameraUpdate_timer.isActive():
            self.autoUpdate()


    def closeEvent(self, event):
        self.camera_thread.exit()
        self.camera_thread.join()
        event.accept()

    ### 2. 视觉图像更新
    def visionUpdateFrame(self,result_flag=True):
        try:
            ### 更新原始图像
            if not self.camera_thread.raw_frame_queue.empty():
                original_img = self.camera_thread.raw_frame_queue.get_nowait()
                original_img = cv2.resize(original_img, (560, 340))
                rgb_original = cv2.cvtColor(original_img, cv2.COLOR_BGR2RGB)
                h, w, ch = rgb_original.shape
                bytes_per_line = ch * w
                qimg_original = QImage(rgb_original.data, w, h, bytes_per_line, QImage.Format_RGB888)
                self.label_oriShow.setAlignment(Qt.AlignCenter)
                self.label_oriShow.setPixmap(QPixmap.fromImage(qimg_original))

                if result_flag:
                    if self.camera_thread.resultInfo is not None:
                        msg = ""
                        for result in self.camera_thread.result_message:
                            msg = msg + self.camera_thread.resultInfo[:2] + result[0] + " "
                        # msg = self.camera_thread.resultInfo
                        self.logVisionResult(msg)
                    
                    if self.camera_thread.parameterInfo is not None:
                        for item in self.camera_thread.parameterInfo:
                            self.logVisionParameter(str(item))

            # ### 更新处理后的图像（如果有）
            # if not self.camera_thread.processed_frame_queue.empty():
            #     result_img = self.camera_thread.processed_frame_queue.get_nowait()
            #     result_img = cv2.resize(result_img, (600, 500))
            #     rgb_result = cv2.cvtColor(result_img, cv2.COLOR_BGR2RGB)
            #     h, w, ch = rgb_result.shape
            #     bytes_per_line = ch * w
            #     qimg_result = QImage(rgb_result.data, w, h, bytes_per_line, QImage.Format_RGB888)
            #     self.label_resultShow.setAlignment(Qt.AlignCenter)
            #     self.label_resultShow.setPixmap(QPixmap.fromImage(qimg_result))

        except Exception as e:
            print(f"Error visionUpdateFrame: {e}")


    def clearVisionParameter(self):
        self.msgVisionPara_history.clear()
        self.textBrowser_visionParam.clear()

    def clearVisionResult(self):
        self.msg_history.clear()
        self.textBrowser_visionResultInfo.clear()

####### 机器视觉界面------End




####  创建表格视图类
class MyTableModel(QAbstractTableModel):
    
    def __init__(self, data=None):
        super().__init__()
        self._data = data or []
        self.headers = ["No.", "变量名", "Modbus地址", "值"]

    def rowCount(self, parent=None):
        return len(self._data)

    def columnCount(self, parent=None):
        return len(self.headers)

    def data(self, index, role=Qt.DisplayRole):
        if not index.isValid():
            return None
        if role == Qt.DisplayRole:
            row = index.row()
            col = index.column()
            if row < len(self._data) and col < len(self._data[row]):
                return self._data[row][col]
        elif role == Qt.TextAlignmentRole:
            return Qt.AlignCenter
        return None

    ### 表格视图-插入数据
    def setData(self, index, value, role=Qt.EditRole):
        if not index.isValid():
            return False
        if role == Qt.EditRole:
            self._data[index.row()][index.column()] = value
            self.dataChanged.emit(index, index, [role])
            return True
        return False

    def headerData(self, section, orientation, role=Qt.DisplayRole):
        if role == Qt.DisplayRole and orientation == Qt.Horizontal:
            return self.headers[section]
        return None

def adjust_row_heights(view):
    for row in range(view.model().rowCount()):
        view.setRowHeight(row, 45) ## 单位：像素



if __name__ == '__main__':

    master = process_Master()
    # master = ModbusMaster()
    # rtu_master = rtu_Master()
    app = QApplication(sys.argv)
    ex = MainWindow()
    # ex.show()
    ex.showMaximized()
    sys.exit(app.exec_())
